package com.grt192.actuator.create;

import simulator.Simulator;

import com.grt192.actuator.exception.ActuatorException;
import com.grt192.core.Actuator;
import com.grt192.core.Command;
import com.grt192.core.CommandArray;

/**
 * CreateDrive actuates IRobotCreate base wheels.
 * 
 * 
 * @author ajc
 */
public class CreateDrive extends Actuator {

	/** Maximum speed the Create can drive */
	public static final int MAX_VELOCITY = 500;

	// yo dawg
	public static class Direction {
		public static final int FORWARD = 1;
		public static final int BACKWARD = 1;
	}

	private Simulator create;
	private int controlMode;

	/**
	 * Constructs a new CreateDrive train, with percent control mode
	 * 
	 * @param create
	 *            robot to actuate
	 */
	public CreateDrive(Simulator sim) {
		this.create = sim;

	}

	/**
	 * Changes the control mode of the DT, changing how it is commanded to drive
	 * 
	 * @param controlMode
	 *            new controlMode
	 */
	public void setControlMode(int controlMode) {
		this.controlMode = controlMode;
	}

	/**
	 * 
	 */
	protected void executeCommand(Command c) throws ActuatorException {
		if (c instanceof CommandArray) {
			CommandArray ca = (CommandArray) c;
			if (create != null) {
				// int left, right;
				switch (controlMode) {
				case CONTROL_DIRECT:
					// left = (int) limit(-MAX_VELOCITY, MAX_VELOCITY,
					// ca.getValue(0));
					// right = (int) limit(-MAX_VELOCITY, MAX_VELOCITY,
					// ca.getValue(1));
					//
					// log("DIRECT: LEFT:\t" + left + "RIGHT:\t" + right);
					// create.driveDirect(left, right);
					create.driveDirect(
							(int) limit(-MAX_VELOCITY, MAX_VELOCITY,
									ca.getValue(0)),
							(int) limit(-MAX_VELOCITY, MAX_VELOCITY,
									ca.getValue(1)));
					break;
				case CONTROL_RADIUS:
					create.drive(
							(int) limit(-MAX_VELOCITY, MAX_VELOCITY,
									(int) ca.getValue(0)), (int) ca.getValue(1));
					break;
				case CONTROL_PERCENT:
					// left = (int) (ca.getValue(0) * MAX_VELOCITY);
					// right = (int) (ca.getValue(1) * MAX_VELOCITY);
					// log("\t\t\tPERCENT: LEFT:\t" + left + "RIGHT:\t" +
					// right);
					// create.driveDirect(left, right);
					create.driveDirect((int) (ca.getValue(0) * MAX_VELOCITY),
							(int) (ca.getValue(1) * MAX_VELOCITY));
					break;
				default:
					// TODO exception
				}
			}

		}

	}

	/**
	 * Stop actuation
	 */
	protected void halt() {
		create.stop();
	}

	private static double limit(int min, int max, double in) {
		if (in < min) {
			return min;
		}
		if (in > max) {
			return max;
		}
		return in;

	}

}
